/**
 * @file odom_serial_task.c/h
 * @author 周鸣阳
 * @brief nano通信处理任务线程,发送陀螺仪及底盘数据
 * @version 0.1
 * @date 2022-03-14
 * 
 * @copyright Copyright (c) 2022 SPR
 * 
 */
#include "main.h"
#include "cmsis_os.h"

#include "chassis_task.h"
#include "odom_serial_task.h"
#include "INS_task.h"
#include "bsp_usart.h"
#include "bsp_rc.h"
#include "CRC8_CRC16.h"
#include "fifo.h"
#include "protocol.h"
#include "usart.h"
#include "board_talk.h"

const AGV_t *agv_data;
const INS_data_t *ins_data;

odom_u odom_data[ODOM_Data_Total];

uint8_t send_data[SINGLE_DATA_LENGTH*ODOM_Data_Total + 5]; //两个起始位与一个结束位和两个校验位

u8 buf[4] = {0,0,0,0};

void odom_serial_task(void const *pvParameters)
{
	vTaskDelay(20);
	agv_data = get_agv_move_point();
	ins_data = get_INS_data_point();   //仅有加速度计和陀螺仪

    while (1)
    {
		odom_data[VX].f = agv_data->chassis_driver.vx;
		odom_data[VY].f = agv_data->chassis_driver.vy;
		odom_data[VTH].f = agv_data->chassis_driver.wz;//ins_data->wz;
		//C板安装位置影响
		odom_data[AX].f = ins_data->ay;
		odom_data[AY].f = -ins_data->ax;
		odom_data[WZ].f = ins_data->wz;
		
		send_data[0] = 0xA5;
		send_data[1] = 0x5A;
		
		for(int i=0,n=2;i<ODOM_Data_Total;++i)
		{
			for(int j=0;j<SINGLE_DATA_LENGTH;++j)
			{
				send_data[n++] = odom_data[i].s[j];
			}
		}
		send_data[SINGLE_DATA_LENGTH*ODOM_Data_Total + 2] = 0xFE;
		append_CRC16_check_sum(send_data,sizeof(send_data));
		
		usart6_tx_dma_enable(send_data,sizeof(send_data));

        vTaskDelay(5);
    }
}


